package com.bangdao.hu.roomba;


import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;



import android.bluetooth.BluetoothSocket;
import android.os.Handler;
import android.os.Message;
import android.util.Log;



public class ConnectedThread extends Thread {
    private static final String ConnectedThreadTag = "ConnectedThreadTag";
	private final BluetoothSocket mmSocket;
    private final InputStream mmInStream;
    private final OutputStream mmOutStream;
    /** current speed for movement operations that don't take a speed */
    public int speed = defaultSpeed;
    /** default speed for movement operations if speed isn't specified */
    public static final int defaultSpeed  =  200;
    /** current mode, if known */
    int mode;
    /** if sensor variables have been updated successfully */
    protected boolean sensorsValid = false;
    /** how many bytes we expect to read from the sensor command */
    protected int readRequestLength;
    RobotConnection robotConnection;
    /** internal storage for all roomba sensor data */
    protected byte[] sensor_bytes = new byte[1024];
    /** last time (System.currentTimeMillis) that the sensors were updated */
    protected long sensorsLastUpdateTime;
	public Handler handler;
    
 // possible modes
    public static final int MODE_UNKNOWN = 0;
    public static final int MODE_PASSIVE = 1;
    public static final int MODE_SAFE    = 2;
    public static final int MODE_FULL    = 3;

    // Roomba ROI opcodes
    // these should all be bytes, but Java bytes are signed, sucka
    public static final int START   =  128;  // 0
    public static final int BAUD    =  129;  // 1
    public static final int CONTROL =  130;  // 0
    public static final int SAFE    =  131;  // 0
    public static final int FULL    =  132;  // 0
    public static final int POWER   =  133;  // 0
    public static final int SPOT    =  134;  // 0
    public static final int CLEAN   =  135;  // 0
    public static final int MAX     =  136;  // 0
    public static final int DRIVE   =  137;  // 4
    public static final int MOTORS  =  138;  // 1
    public static final int LEDS    =  139;  // 3
    public static final int SONG    =  140;  // 2N+2
    public static final int PLAY    =  141;  // 1
    public static final int SENSORS =  142;  // 1
    public static final int DOCK    =  143;  // 0
    public static final int PWMMOTORS = 144; // 3
    public static final int DRIVEWHEELS = 145; 	// 4
    public static final int DRIVEPWM = 146;  // 4
    public static final int STREAM  =  148;  // N+1
    public static final int QUERYLIST = 149; // N+1
    public static final int STOPSTARTSTREAM = 150;  // 1
    public static final int WAITDISTANCE  = 156;  // wait distance 2 data bytes
    public static final int WAITANGLE  = 157;  // wait angle 2 data bytes
    
    public static final int SCHEDULINGLEDS = 162; 	// 2
    public static final int DIGITLEDSRAW = 163; 	// 4
    public static final int DIGITLEDSASCII = 164;	// 4
    public static final int BUTTONSCMD  =  165; // 1
    public static final int SCHEDULE =  167;  // n
    public static final int SETDAYTIME = 168; // 3
    /** distance between wheels on the roomba, in millimeters */
    public static final int wheelbase = 258;
    /** mm/deg is circumference distance divided by 360 degrees */
    public static final float millimetersPerDegree = (float)(wheelbase * Math.PI / 360.0);
    
    
    // offsets into sensor_bytes data
    public static final int BUMPSWHEELDROPS     = 0;
    public static final int WALL                = 1;
    public static final int CLIFFLEFT           = 2;
    public static final int CLIFFFRONTLEFT      = 3;
    public static final int CLIFFFRONTRIGHT     = 4;
    public static final int CLIFFRIGHT          = 5;
    public static final int VIRTUALWALL         = 6;
    public static final int MOTOROVERCURRENTS   = 7;
    public static final int DIRTLEFT            = 8;
    public static final int DIRTRIGHT           = 9;
    public static final int REMOTEOPCODE        = 10;
    public static final int BUTTONS             = 11;
    public static final int DISTANCE_HI         = 12;
    public static final int DISTANCE_LO         = 13;  
    public static final int ANGLE_HI            = 14;
    public static final int ANGLE_LO            = 15;
    public static final int CHARGINGSTATE       = 16;
    public static final int VOLTAGE_HI          = 17;
    public static final int VOLTAGE_LO          = 18;  
    public static final int CURRENT_HI          = 19;
    public static final int CURRENT_LO          = 20;
    public static final int TEMPERATURE         = 21;
    public static final int CHARGE_HI           = 22;
    public static final int CHARGE_LO           = 23;
    public static final int CAPACITY_HI         = 24;
    public static final int CAPACITY_LO         = 25;

    // bitmasks for various thingems
    public static final int WHEELDROP_MASK      = 0x1C;
    public static final int BUMP_MASK           = 0x03;
    public static final int BUMPRIGHT_MASK      = 0x01;
    public static final int BUMPLEFT_MASK       = 0x02;
    public static final int WHEELDROPRIGHT_MASK = 0x04;
    public static final int WHEELDROPLEFT_MASK  = 0x08;
    public static final int WHEELDROPCENT_MASK  = 0x10;

    public static final int MOVERDRIVELEFT_MASK = 0x10;
    public static final int MOVERDRIVERIGHT_MASK= 0x08;
    public static final int MOVERMAINBRUSH_MASK = 0x04;
    public static final int MOVERVACUUM_MASK    = 0x02;
    public static final int MOVERSIDEBRUSH_MASK = 0x01;

    public static final int POWERBUTTON_MASK    = 0x08;  
    public static final int SPOTBUTTON_MASK     = 0x04;  
    public static final int CLEANBUTTON_MASK    = 0x02;  
    public static final int MAXBUTTON_MASK      = 0x01;  

    // which sensor packet, argument for sensors(int)
    public static final int SENSORS_ALL         = 0;
    public static final int SENSORS_PHYSICAL    = 1;
    public static final int SENSORS_INTERNAL    = 2;
    public static final int SENSORS_POWER       = 3;

    public static final int REMOTE_NONE         = 0xff;
    public static final int REMOTE_POWER        = 0x8a;
    public static final int REMOTE_PAUSE        = 0x89;
    public static final int REMOTE_CLEAN        = 0x88;
    public static final int REMOTE_MAX          = 0x85;
    public static final int REMOTE_SPOT         = 0x84;
    public static final int REMOTE_SPINLEFT     = 0x83;
    public static final int REMOTE_FORWARD      = 0x82;
    public static final int REMOTE_SPINRIGHT    = 0x81;
    public volatile static boolean Robot_Status_Stop = false;
    private byte allBytes[] = new byte[1024]; 
 
    public static final int MSG_MOVE = 0x00;
    public static final int MSG_MOVE_SAMPLE = 0x01;
    public static final int MSG_DELAY = 0x02;
    
    public Handler bHandler = new Handler(){
    		public void handleMessage(Message message){
	    		switch (message.what){
	    		case MSG_MOVE:
	    			newDrive(2000);
	    			break;
	    		case MSG_MOVE_SAMPLE:
	    			newDrive(2000);
	    			break;
	    		case MSG_DELAY:
	    			try {
						Thread.sleep(1000);
					} catch (InterruptedException e) {
						// TODO Auto-generated catch block
						e.printStackTrace();
					}
	    			break;
	    			
    		}
    		}};
 
    public ConnectedThread(BluetoothSocket socket) {
        mmSocket = socket;
        InputStream tmpIn = null;
        OutputStream tmpOut = null;
 
        // Get the input and output streams, using temp objects because
        // member streams are final
        try {
            tmpIn = socket.getInputStream();
            tmpOut = socket.getOutputStream();
        } catch (IOException e) { }
 
        mmInStream = tmpIn;
        mmOutStream = tmpOut;
    }
 
    public void run() {
//        byte[] buffer = new byte[1024];  // buffer store for the stream
//        int bytes; // bytes returned from read()
// 
//        // Keep listening to the InputStream until an exception occurs
//        while (true) {
//            try {
//            	 System.out.println("reading buffer");
//                // Read from the InputStream
//                bytes = mmInStream.read(buffer);
//                allBytes = buffer;
//                System.out.println(bytes);
//                // Send the obtained bytes to the UI activity
////                mHandler.obtainMessage(1, bytes, -1, buffer);
//            } catch (IOException e) {
//                break;
//            }
//        }
    }
    
    public byte[] getBytes()
    {
    	byte[] buffer = new byte[1024];
    	int bytes;
    	try {
			bytes = mmInStream.read(buffer);
		} catch (IOException e) {
			e.printStackTrace();
		}
    	return buffer;
    }
     
 
    /* Call this from the main activity to send data to the remote device */
    public void send(byte[] bytes) {
        try {
            mmOutStream.write(bytes);
        } catch (IOException e) { }
    }
    
    /* Call this from the main activity to send data to the remote device */
    public void send(int b) {
        try {
            mmOutStream.write(b);
        } catch (IOException e) { }
    }
 
    /* Call this from the main activity to shutdown the connection */
    public void cancel() {
        try {
            mmSocket.close();
        } catch (IOException e) { }
    }
    
    public void startup() {
        logmsg("startup");
        speed = defaultSpeed;
        start();
    }

    /** 
     * Reset Roomba after a fault.  This takes it out of whatever mode it was
     * in and puts it into safe mode.
     * This command also syncs the object's sensor state with the Roomba's
     * by calling updateSensors()
     * @see #startup()
     * @see #updateSensors()
     */
    public void reset() {
        logmsg("reset");
        stopDrive();
        startup();
        control();
        updateSensors();
    }

    /**  Send START command  */
    public void start() { 
        logmsg("start");
        mode = MODE_PASSIVE;
        send( START );
    }
    /**  Send CONTROL command  */
    public void control() { 
        logmsg("control");
        mode = MODE_SAFE;
        send( CONTROL );
        // set blue dirt LED on so we know roomba is powered on & under control
        // (and we don't forget to turn it off, and run it's batteries flat)
        // FIXME: first time after a poweron, the lights flash then turn off
        setLEDs(false, false, false, false, false, true, 128, 255);
    }
    
    /** 
     * Turns on/off the various LEDs.
     * Low-level command.
     * FIXME: this is too complex
     */
    public void setLEDs( boolean status_green, boolean status_red, 
                         boolean spot,boolean clean,boolean max,boolean dirt, 
                         int power_color, int power_intensity ) {
        int v = (status_green?0x20:0) | (status_red?0x10:0) | 
            (spot?0x08:0) | (clean?0x04:0) | (max?0x02:0) | (dirt?0x01:0);
        logmsg("setLEDS: ");
        byte cmd[] = { (byte)LEDS, (byte)v,
                       (byte)power_color, (byte)power_intensity };
        send(cmd);
    }
    
    /**  Send SAFE command  */
    public void safe() { 
        logmsg("safe");
        mode = MODE_SAFE;
        send( SAFE );
    }
    /**  Send FULL command  */
    public void full() { 
        logmsg("full");
        mode = MODE_FULL;
        send( FULL );
    }

    /**
     * Power off the Roomba.  Once powered off, the only way to wake it
     * is via wakeup() (if implemented) or via a physically pressing
     * the Power button
     * @see #wakeup()
     */
    public void powerOff() {
        logmsg("powerOff");
        mode = MODE_UNKNOWN;
        send( POWER );
    }

    /** Send the SPOT command */
    public void spot() {
        logmsg("spot");
        mode = MODE_PASSIVE;
        send( SPOT );
    }
    /** Send the CLEAN command */
    public void clean() {
        logmsg("clean");
        mode = MODE_PASSIVE;
        send( CLEAN );
    }
    /** Send the max command */
    public void max() {
        logmsg("max");
        mode = MODE_PASSIVE;
        send( MAX );
    }
    /** Send the max command */
    public void dock() {
        logmsg("dock");
        mode = MODE_PASSIVE;
//        send( CLEAN );
        send( DOCK );
    }
    
    /** 
     * Distance traveled since last requested
     * units: mm
     * range: -32768 - 32767
     */
    public short distance() {
        return toShort(sensor_bytes[DISTANCE_HI],
                       sensor_bytes[DISTANCE_LO]);
    }
    
    /**
    *
    */
   static public final short toShort(byte hi, byte lo) {
       return (short)((hi << 8) | (lo & 0xff));
   }
    
    /** 
     * Send the SENSORS command 
     * with one of the SENSORS_ arguments
     * Typically, one does "sensors(SENSORS_ALL)" to get all sensor data
     * @param packetcode one of SENSORS_ALL, SENSORS_PHYSICAL, 
     *                   SENSORS_INTERNAL, or SENSORS_POWER, or for roomba 5xx, it
     *                   is the sensor packet number (from the spec)
     */
    public void sensors(int packetcode ) {
    	sensorsValid = false;
        logmsg("sensors:"+packetcode);
        switch (packetcode) {
        case 0: readRequestLength = 26; break;
        case 1: readRequestLength = 10; break;
        case 2: readRequestLength = 6; break;
        case 3: readRequestLength = 10; break;
        case 4: readRequestLength = 14; break;
        case 5: readRequestLength = 12; break;
        case 6: readRequestLength = 52; break;
        case 100: readRequestLength = 80; break;
        case 101: readRequestLength = 28; break;
        case 106: readRequestLength = 12; break;
        case 107: readRequestLength = 9; break;
        case 19:
        case 20:
        case 22:
        case 23:
        case 25:
        case 26:
        case 27:
        case 28:
        case 29:
        case 30:
        case 39:
        case 40:
        case 41:
        case 42:
        case 43:
        case 44:
        case 46:
        case 47:
        case 48:
        case 49:
        case 50:
        case 51:
        case 54:
        case 55:
        case 56:
        case 57: readRequestLength = 2; break;
        default: readRequestLength = 1; break;
        }
        
        byte cmd[] = { (byte)SENSORS, (byte)packetcode};
        send(cmd);
    }
    
    /** 
     * get all sensor data
     */
    public void sensors() {
    	readRequestLength = 26;
        sensors( SENSORS_ALL );
    }
	/**
	 * Read roomba 26-byte sensor record using robotConnection. Tries once to read valid data, allowing 100ms
	 * timeout on each attempt. 
	 * @return true if read 26 bytes of valid data. Data has been stored in sensor_bytes. False otherwise
	 */
    /*
 	public boolean updateSensors()
 	{
 		return updateSensors(SENSORS_ALL);
 	}
 	
	public boolean updateSensors(int sensorGroup)
 	{
    	int sensorGroupSize;
    	
 		if (robotConnection == null) {
 	 		System.out.println("Error at ArduinoBot.updateSensors(): no connection object for robot");
 	 		return false; 			
 		}

 		switch(sensorGroup) {
 		case SENSORS_ALL: sensorGroupSize = 26; break;
 		case 100: sensorGroupSize = 80; break;
 		default:
 			System.err.println("Invalid sensor group in updateSensors(): " + sensorGroup);
 			return false;
 		}
    	sensors(sensorGroup);
 		return getSensorData(sensorGroupSize); 		
}*/

    /**
     * Query a list of sensors. This is a roomba 5xx only command.
     * @param sensorList A byte array containing the sensor groups requested to be read
     * @param returnLen The number of bytes of data expected to be returned from roomba
     */
    public void queryList(byte[] sensorList, int returnLen)
    {
    	int i = 0;
    	int j;
    	
    	sensorsValid = false;
    	readRequestLength = returnLen;
    	byte cmd[] = new byte[2+sensorList.length];
    	cmd[i++] = (byte) QUERYLIST;
    	cmd[i++] = (byte)sensorList.length;
    	for (j=0; j<sensorList.length; j++)
    		cmd[i++] = sensorList[j];
    	send(cmd);
    }

	/**
	 * @param sensorGroupSize
	 */
	public boolean getSensorData(int sensorGroupSize) {
 		byte [] readData;
 		
		// try once to read valid sensor data before giving up
		//startTime = System.currentTimeMillis();
		try {
			readData = robotConnection.readBot(sensorGroupSize);
		} catch (Exception e) {
			e.printStackTrace();
			return false;
		}

		// readBot either read requested # of bytes or returned a null (indicating timeout. 
		// If null or invalid data and group 0 (26 bytes expected), try again
		if( readData != null ) { 
			if (((readData[1] > 1) || (readData[1] < 0)) && (sensorGroupSize == 26)) {
				sensorsValid = false;
				logmsg("updateSensors: received invalid data while attempting to read Roomba sensors!");
			} else {
				sensorsValid = true;
				System.arraycopy(readData, 0, sensor_bytes, 0, sensorGroupSize);
				logmsg("updateSensors: sensorsValid!");            		
				//elapsedTime = System.currentTimeMillis() - startTime;
				return true;
			}
        }
		System.out.println("Error: timeout on sensor read");
		return false;
	}
    //
    // basic functions
    //

    /** 
     * Alias to pause
     * @see #pause(int)
     */
    public void delay( int millis ) {  pause( millis );  }

    /** 
     * Just a simple pause function. 
     * Makes the thread block with Thread.sleep()
     * @param millis number of milliseconds to wait
     */
    public void pause( int millis ) {
        try { Thread.sleep(millis); } catch(Exception e) { }
    }


    //
    // higher-level functions
    //

    /**
     * Stop Rooomba's motion.
     * Sends drive(0,0)
     */
    public void stopDrive() {
        logmsg("stop");
        drive( 0, 0 );
    }

    /** Set speed for movement commands */
    public void setSpeed( int s ) { speed = Math.abs(s); }
    /** Get speed for movement commands */
    public int  getSpeed() { return speed; }

    /**
     * Go straight at the current speed for a specified distance.
     * Positive distance moves forward, negative distance moves backward.
     * This method blocks until the action is finished.
     * @param distance distance in millimeters, positive or negative
     */
    public void goStraight( int distance ) {
        float pausetime = Math.abs(distance / speed);  // mm/(mm/sec) = sec
        if (distance > 0)
        	goStraightAt( speed );
        else
        	goStraightAt( -speed);
        
        //TUDO:  roomba move straight is not accurate
//        for()
//        {
//        	
//        	
//        }
        
        if(distance >= 2000)
        {
        	pause( (int)((0.78*pausetime)*1000) );
        }else
        {
        	pause( (int)((pausetime)*1000) );
        }
        stopDrive();
    }
    
    
    
    /**
     * Spin left a specified angle at a specified speed
     * @param angle angle in degrees, positive
     */
    public void spinLeft( int angle ) {
        if( angle<0 ) return;
        //float pausetime = 
        float pausetime = Math.abs( millimetersPerDegree * angle / speed );
        spinLeftAt( Math.abs(speed) );
        pause( (int)(pausetime*1000) );
        stopDrive();
    }
    
    /**
     * Spin right the current speed for a specified angle 
     * @param angle angle in degrees, positive
     */
    public void spinRight( int angle ) {
        if( angle < 0 ) return;
        float pausetime = Math.abs( millimetersPerDegree * angle / speed );
        spinRightAt( Math.abs(speed) );
        pause( (int)(pausetime*1000) );
        stopDrive();
    }
    
    /** 
     * Spin in place anti-clockwise, at the current speed
     */
    public void spinLeft() {
        spinLeftAt( speed );  
    }
    /** 
     * Spin in place clockwise, at the current speed
     */
    public void spinRight() {
        spinRightAt( speed );  
    }
    
    /**
     * Spin right or spin left at a particular speed
     * @param speed to spin at, 
     *              positive to spin left, negative to spin right
     */
    public void spinAt( int speed ) {
        if( speed > 0 )       spinLeftAt( speed );
        else if( speed < 0 )  spinRightAt( -speed );
    }

    /**
     * Spin in place anti-clockwise, at the current speed.
     * @param aspeed speed to spin at
     */
    public void spinLeftAt(int aspeed) {
        drive( aspeed, 1 ); 
    }

    /**
     * Spin in place clockwise, at the current speed.
     * @param aspeed speed to spin at, positive
     */
    public void spinRightAt(int aspeed) {
        drive( aspeed, -1 ); 
    }
    
    /** 
     * Go straight at a specified speed.  
     * Positive is forward, negative is backward
     * @param velocity velocity of motion in mm/sec
     */
    public void goStraightAt( int velocity ) {
        //System.out.println("goStraightAt: velocity:"+velocity);
        if( velocity > 500 ) velocity = 500;
        if( velocity < -500 ) velocity = -500;
        drive( velocity, 0x8000 );
    }

    public void newDrive(int distance)
    {
    	Robot_Status_Stop = false;
    	double startTime = System.currentTimeMillis();
    	if(distance >= 0)
    		drive( speed, 0x8000 );
    	else
    		drive(-speed, 0x8000);
    	waitDistance(distance);
    	stopDrive();
    	try {
			Thread.sleep(2000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    	double endTime  = System.currentTimeMillis();
    
    	Robot_Status_Stop = true;
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    	System.out.println("Time elapsed is " + (endTime-startTime));
    }
    
    
//    while(!stop)
//    {
//    	//wait till it stopped
//    	
//    }
    
    
    
    
    
    
    
    public void newTurn(int degree)
    {
    	try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    	if(degree > 0)
    	{
	    	drive(200, 1);
    	}else{
    		drive(200, -1);
    	}
    	waitAngle(degree);
    	stopDrive();
    }
    
    public void waitDistance(int distance)
    {
    	byte[] cmd = {(byte)WAITDISTANCE, (byte)(distance >> 8), (byte) distance};
    	send(cmd);
    }
    
    public void waitAngle(int angle)
    {
    	byte[] cmd = {(byte)WAITANGLE, (byte)(angle >> 8), (byte) angle};
    	send(cmd);
    }

    /**
     * @param distance distance in millimeters, positive 
     */
    public void goForward( int distance ) {
        if( distance < 0 ) return;
        goStraight( distance );
    }

    /**
     * @param distance distance in millimeters, positive 
     */
    public void goBackward( int distance ) {
        if( distance < 0 ) return;
        goStraight( -distance );
    }

    /**
     *
     */
    public void turnLeft() {
        turn(129);
    }
    public void turnRight() {
        turn(-129);
    }
    public void turn( int radius ) {
        drive( speed, radius );
    }

    public boolean updateSensors()
 	{
 		return updateSensors(SENSORS_ALL);
 	}
    /**
     * Update sensors.  Block for up to 1000 ms waiting for update
     * To use non-blocking, call sensors() and then poll sensorsValid()
     */

    
    /**
     * Update sensors.  Block for up to 1000 ms waiting for update
     * To use non-blocking, call sensors() and then poll sensorsValid()
     */
    public boolean updateSensors(int packetcode) {
        sensorsValid = false;
        sensors(packetcode);
        for(int i=0; i < 20; i++) {
            if( sensorsValid ) { 
                logmsg("updateSensors: sensorsValid!");
                break;
            }
            logmsg("updateSensors: pausing...");
            pause( 50 );
        }

        return sensorsValid;
    }

    /**
     * called by serialEvent when we have enough bytes to make sensors valid
     */
    public void computeSensors() {
        sensorsValid = true;
        sensorsLastUpdateTime = System.currentTimeMillis();
        computeSafetyFault();
    }
    
    /**
     * Compute possible safety fault.
     * Called on every successful updateSensors().
     * In normal use, call updateSensors() then check safetyFault().
     * @return  true if indicates we had an event that took the Roomba out of
     *          safe mode
     * @see #updateSensors()
     */
    public boolean computeSafetyFault() {
        safetyFault = (sensor_bytes[BUMPSWHEELDROPS] & WHEELDROP_MASK) != 0 ||
            sensor_bytes[CLIFFLEFT]==1  || sensor_bytes[CLIFFFRONTLEFT]==1 ||
            sensor_bytes[CLIFFRIGHT]==1 || sensor_bytes[CLIFFFRONTRIGHT]==1;

        if( safetyFault && (mode == MODE_SAFE) ) mode = MODE_PASSIVE;

        return safetyFault;
    }
    
    /** computed boolean for when Roomba is errored out of safe mode */
    boolean safetyFault = false;
    
    /**
     * Move the Roomba via the low-level velocity + radius method.
     * See the 'Drive' section of the Roomba ROI spec for more details.
     * Low-level command.
     * @param velocity  speed in millimeters/second, 
     *                  positive forward, negative backward
     * @param radius    radius of turn in millimeters
     */
    public void drive( int velocity, int radius ) {
        byte cmd[] = { (byte)DRIVE,(byte)(velocity>>>8),(byte)(velocity&0xff), 	// what does >>> mean?
                       (byte)(radius >>> 8), (byte)(radius & 0xff) };
//        logmsg("drive: "+hex(cmd[0])+","+hex(cmd[1])+","+hex(cmd[2])+","+
//               hex(cmd[3])+","+hex(cmd[4]));
        send( cmd );
    }
    
    private void logmsg(String s)
    {
    	Log.i(ConnectedThreadTag,s);
    }

    
}


//TODO: lookup wait distance !!!!!!!!



